#include "servo.h"
#include "tim.h"

const int servoMaxAnlge = 270;
		
int servo_init(void)
{
    // Set up the servo pin as output
    servo1.writeValue(0);
    servo1.state = SERVO_STATE_DISABLE;
    servo1.writeEnable(1);
    return 0;
}

/**
 * @brief set compare value
 * @param value 0~1000
 */
int servo_set_value(uint32_t value)
{
    /* T = 20ms, compare = 0.5ms~2.5ms
       angle 0~180 <=> value 0.5/20*10000~2.5/20*10000 => 250~1250
    */
    // Set the servo to the specified value
    if (value > 1000)
        return -1;
    const uint32_t cnt_freq = 10000;
    uint32_t auto_reload = __HAL_TIM_GetAutoreload(&htim1);
    uint32_t compare_value = (uint16_t)(auto_reload * (float)(value + 250) / cnt_freq + 0.5f);
    __HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_4, compare_value);
    servo1.value = value;
    servo1.angle = (float)value * 180 / 1000;
    return 0;
}

/**
 * @brief set angle
 * @param angle 0~180
 */
int servo_set_angle(float angle)
{
    // Set the servo to the specified angle
    if (angle > servoMaxAnlge || angle < 0)
        return -1;
    uint32_t value = (uint32_t)(angle * 1000 / servoMaxAnlge + 0.5f);
    servo_set_value(value);
    return 0;
}

int servo_set_enable(int state)
{
    if ((state == 0 && (servo1.state == SERVO_STATE_ENABLE)) ||
        (state != 0 && (servo1.state == SERVO_STATE_DISABLE)))
    {
        if (state)
        {
            HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_4);
            servo1.state = SERVO_STATE_ENABLE;
        }
        else
        {
            HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_4);
            servo1.state = SERVO_STATE_DISABLE;
        }
        return 0;
    }
    return -1;
}

servo_t servo1 =
{
    .value = 0,
    .angle = 0,
    .init = servo_init,
    .writeAngle = servo_set_angle,
    .writeValue = servo_set_value,
    .writeEnable = servo_set_enable,
};
